#include "modules/CtrlModule/PassivePlanner/TiltReactionPlanner.h"
#include "lib/WrapFuncs.h"
#include "modules/CtrlModule/MarkPosePubModel.h"

namespace behavior_controller
{
    #define TURN_REEOR 0.052359876
    TiltReactionPlanner tilt_reaction_planner;
    void TiltReactionPlanner::run(double &linear_vel, double &angular_vel)
    {
        if (DataSet::imu_data.max_abs_tilt()>= MIN_TILT) //后退到去除触发倾角设定
        {
            Back(linear_vel, angular_vel);
            last_angle = DataSet::robot_pose.angle();           
        }
        else
        {
            static bool auto_= true;
            if(auto_)
            {
                TurnToAngle(linear_vel, angular_vel);
            }
            else
            {
                Turn(linear_vel, angular_vel);
            }
            
        }
    }

    void TiltReactionPlanner::Back(double &linear_vel, double &angular_vel)
    {
        linear_vel = 0.2*linear_vel_sig;
        angular_vel = 0;
    }

    void TiltReactionPlanner::Turn(double &linear_vel, double &angular_vel)
    {
        static double turn_angle_tmp=  M_PI/4; 
        if(fabs(Funcs::NormalizeAngle(DataSet::robot_pose.angle()-last_angle))> turn_angle_tmp)
        {
            finished = true;
        }
        else
        {
            angular_vel = 0.4*angular_vel_sig; 
            linear_vel = 0;
        }
    }
    void TiltReactionPlanner::TurnToAngle(double &linear_vel, double &angular_vel)
    {
        if(!finished_turn)
        {
            double dst_angle= Funcs::NormalizeAngle(DataSet::robot_pose.angle() - global_goal_angle);
            std::cout<< "TurnToAngle  dst_angle= "<< dst_angle  << "  global_goal_angle= "<< global_goal_angle << "  robot_pose.angle= "<< DataSet::robot_pose.angle()  <<std::endl;  
            if(fabs(dst_angle) >= TURN_REEOR)  // 0.052359876rad ~= 3.0deg
            {
                if (dst_angle > 0.17 || dst_angle < -0.17) //当角度差相差0.1弧度时    1deg=0.017453292rad
                {
                        linear_vel = 0;
                        angular_vel = -dst_angle;
                }
                 else if (dst_angle > TURN_REEOR &&  dst_angle < 0.17) //当角度差小于0.1弧度时,大于0.03弧度时 
                {
                        linear_vel = 0;
                        angular_vel = -0.12;
                }
                else if (dst_angle > -0.17 && dst_angle < -TURN_REEOR) //当角度差小于0.1弧度时,大于0.052359876弧度(~3度)时
                {
                        linear_vel = 0;
                        angular_vel = 0.12;
                }
                else
                {
                        linear_vel = 0;
                        angular_vel = 0;
                }
                return;
            }
            else
            {
                finished_turn=false;
                finished = true;
            }
        }
    }

}